/*
 * @Author: yuchen 986138976@qq.com
 * @Date: 2022-11-20 19:20:00
 * @LastEditors: yuchen 986138976@qq.com
 * @LastEditTime: 2022-12-04 20:16:31
 * @FilePath: \MDK-ARM\RM3508\RM3508.h
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
/*********************************************************************************
 *@函数功能简介：
 *@	RM3508_Set_I：			RM3508电流控制，需要选择CAN号
 *@	RM3508_Set_Speed：		RM3508速度闭环，通过电流作为输出
 *@	RM3508_Set_Pos：		RM3508位置闭环，通过速度作为输出，串级PID
 *@
 *@	RM3508_Get_Feedback：	RM3508数据反馈获取，CAN中断中使用
 *@	M_2006_Get_Torque：		RM3508转矩信息获取
 *@	RM3508_Get_Speed：		速度信息获取
 *@	RM3508_Get_Pos：		位置信息获取
 *@	pos_rec：				位置信息更新，CAN中断中使用，以足够高的采样率解决边界问题
 *@
 *@	Ang2Cnt：				角度制数据转化为编码器的Cnt值
 *@	Cnt2Ang：				编码器Cnt值转化为实际转过的角度
 *@
 *@	RM2006和3508互通的，区别只是电机减速比不同以及3508多了一个电机温度反馈
 *@	因此该文件修改电机的减速比后可以直接用于2006的控制
 *********************************************************************************/

#ifndef __RM3508_H
#define __RM3508_H
#include "can.h"
#include "string.h"
#include "stdint.h"
#include "math.h"
#include "stm32f4xx_hal_can.h"
#define RM3508_CNT_PER_ROUND (8192) // 编码器线程
#define RM3508_CNT_PER_ROUND_OUT(x) (RM3508_CNT_PER_ROUND * RM3508_Reduction_Ratio[(x - 1)])
#define RM3508_CLAMP(x, lower, upper) (x >= upper ? upper : (x <= lower ? lower : x))
#define RM3508_ABS(x) (x >= 0 ? x : -x)
#define CLAMP(x, lower, upper) (x >= upper ? upper : (x <= lower ? lower : x))

#define LIMIT(x, limit) (x >= limit ? limit : (x <= -limit ? -limit : x))
#define DEADBAND(x, limit) (x >= limit ? x : (x <= -limit ? x : 0))
#define INITIATOR(x, limit) (x >= limit ? x : (x <= -limit ? x : (x == 0 ? 0 : (x > 0 ? limit : -limit))))

#define mode_Close 0
#define mode_I 1
#define mode_Speed 2
#define mode_Pos 3

typedef struct
{
	float Target;
	float NowIS;

	float Err;
	float Err_last;
	float Err_sum;
	float I_Start_Err;

	float Kp;
	float Ki;
	float Kd;

	int Max;
	int Min;

	float P_Out;
	float I_Out;
	float D_Out;
	float PID_Out;

	float DeadBand;
	float IntegralLimit;
} M3508_PID;

typedef struct
{
	uint8_t Mode[8]; // 电机模式
	int Target[8];	 // 目标值
} RM3508_State_t;

void RM3508_Set(uint8_t mode, float target, uint8_t ID);
void RM3508_PID_Run(uint8_t ID);
void RM3508_Stop(void);
void RM3508_resume(void);

// 速度PID存储
extern M3508_PID M3508_Speed_Pid[8];
// 位置PID存储
extern M3508_PID M3508_Pos_Pid[8];

// 电流环（在这一环向CAN发数据）
void RM3508_Set_I(int target_i, uint8_t motor_id);
// 通过速度PID设置速度
void RM3508_Set_Speed(int goal_speed, int ID);
// 通过位置PID设置位置（角度位置，是经过减速箱后的位置,PID中debug时存储的Target是cnt）
void RM3508_Set_Pos(float angle_t_goal, int ID);
/*将3508电机的当前值设置为任意位置*/
void RM3508_Set_NowPos(uint8_t ID, int32_t Pos_Angle);

// 获取电机转矩信息
int RM3508_Get_Torque(uint8_t motor_id);
// 获取电机速度信息
int RM3508_Get_Speed(uint8_t motor_id);
// 获取电机位置信息
int RM3508_Get_Pos(uint8_t motor_id);
void RM3508_Set_Pos_Encoder(float angle, int ID);
// 获取电机温度信息
uint8_t RM3508_Get_Temp(uint8_t id);

// 角度转线数
int RM3508_Ang2Cnt(float angle, int ID);
// 线数转角度
float RM3508_Cnt2Ang(int32_t cnt, int ID);

// 解析CAN中的信息（放在中断回调函数中）
void RM3508_Get_Feedback(uint32_t std_id, uint8_t *data_p);

// 发送信号到不同CAN线上
void RM3508_CAN_Send_Data(CAN_HandleTypeDef *hcan);
// CAN中断中实时更新位置信息
static void RM3508_Pos_Rec(uint8_t motor_id);

// 错误函数
static void Error_RM3508(uint8_t error);

#endif
